#include "rclcpp/rclcpp.hpp"

#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <ipa_building_msgs/action/find_room_sequence_with_checkpoints.hpp>
#include <ipa_building_msgs/action/map_segmentation.hpp>
#include "rclcpp_action/rclcpp_action.hpp"
#include <ipa_building_navigation/SimpleActionClientD.h>

using ClientSegmentation=ipa_building_msgs::action::MapSegmentation;
using ClientSequence=ipa_building_msgs::action::FindRoomSequenceWithCheckpoints;


int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto nh = rclcpp::Node::make_shared("room_sequence_planning_client");

    SimpleActionClientD<ClientSegmentation> room_segmentation_client(nh, std::string("room_segmentation_server"));
    SimpleActionClientD<ClientSequence> room_sequence_client(nh, std::string("room_sequence_planning_server"));

    std::vector<std::string> map_names;
    map_names.push_back("map2.pgm");
//    map_names.push_back("lab_ipa.png");
//	map_names.push_back("freiburg_building101.png");
//    map_names.push_back("freiburg_building52.png");
//    map_names.push_back("freiburg_building79.png");
//	map_names.push_back("intel_map.png");
//	map_names.push_back("lab_a.png");
//	map_names.push_back("lab_b.png");
//    map_names.push_back("lab_c.png");
//    map_names.push_back("lab_d.png");
//	map_names.push_back("lab_e.png");
    for (size_t image_index = 0; image_index < map_names.size(); ++image_index) {
        std::string image_filename =
                "/home/ubuntu/ros2_ws/src/Sweeping_robot/ipa_coverage_planning/ipa_room_segmentation/common/files/test_maps/" +
                map_names[image_index];
        std::cout << "current pic is : " << image_filename << std::endl;
        cv::Mat map = cv::imread(image_filename.c_str(), 0);
        //make non-white pixels black
        for (int y = 0; y < map.rows; y++) {
            for (int x = 0; x < map.cols; x++) {
                //find not reachable regions and make them black
                /* if (map.at<unsigned char>(y, x) != 255)
                {
                    map.at<unsigned char>(y, x) = 0;
                } */
                if (map.at<unsigned char>(y, x) < 250) {
                    map.at<unsigned char>(y, x) = 0;
                }
                    //else make it white
                else {
                    map.at<unsigned char>(y, x) = 255;
                }
            }
        }
        sensor_msgs::msg::Image map_msg;
        cv_bridge::CvImage cv_image;
        //	cv_image.header.stamp = ros::Time::now();
        cv_image.encoding = "mono8";
        cv_image.image = map;
        cv_image.toImageMsg(map_msg);
        // create the action client --> "name of server"
        // true causes the client to spin its own thread

        // wait for the action server to start
        while (rclcpp::ok() && !room_segmentation_client.wait_server(5)) {
            std::cout << "room_segmentation_client Action server not available after waiting, wait 5s..." << std::endl;
        }
        // set algorithm parameters
        printf("Action server started, sending goal.\n");
        //DynamicReconfigureClient drc_seg(nh, "/room_segmentation/room_segmentation_server/set_parameters", "/room_segmentation/room_segmentation_server/parameter_updates");
        //drc_seg.setConfig("room_segmentation_algorithm", 1);

        // send a goal to the action
        ipa_building_msgs::action::MapSegmentation::Goal goal_seg;
        goal_seg.data_source = goal_seg.IMAGE_DATA;
        goal_seg.input_map = map_msg;
        goal_seg.map_origin.position.x = 0;
        goal_seg.map_origin.position.y = 0;
        goal_seg.map_resolution = 0.05;
        goal_seg.return_format_in_meter = false;
        goal_seg.return_format_in_pixel = true;
        std::shared_ptr<ClientSegmentation::Result> result_seg = room_segmentation_client.sendGoal(goal_seg);

        if (result_seg == nullptr) {
            printf("Timeout on room segmentation.\n");
            return -1;
        }
        printf("Finished segmentation successfully!\n");

        // solve sequence problem
        /* printf("Waiting for action server '/room_sequence_planning/room_sequence_planning_server' to start.");
        // wait for the action server to start
        ac_seq.waitForServer(); //will wait for infinite time

        // set algorithm parameters
        printf("Action server started, sending goal_seq.");
        DynamicReconfigureClient drc_seq(nh, "/room_sequence_planning/room_sequence_planning_server/set_parameters", "/room_sequence_planning/room_sequence_planning_server/parameter_updates");
        drc_seq.setConfig("planning_method", 1);
        drc_seq.setConfig("tsp_solver", 3);
        drc_seq.setConfig("return_sequence_map", true);
        drc_seq.setConfig("display_map", true); */

        // send a goal_seg to the action
        ipa_building_msgs::action::FindRoomSequenceWithCheckpoints_Goal goal_seq;
        goal_seq.input_map = map_msg;
        goal_seq.map_resolution = goal_seg.map_resolution;
        goal_seq.map_origin.position.x = goal_seg.map_origin.position.x;
        goal_seq.map_origin.position.y = goal_seg.map_origin.position.y;
        goal_seq.room_information_in_pixel = result_seg->room_information_in_pixel;
        goal_seq.robot_radius = 0.3;
        cv::Mat map_eroded;
        cv::erode(map, map_eroded, cv::Mat(), cv::Point(-1, -1), goal_seq.robot_radius / goal_seq.map_resolution + 2);
        cv::Mat distance_map;    //variable for the distance-transformed map, type: CV_32FC1
        cv::distanceTransform(map_eroded, distance_map, CV_DIST_L2, 5);
        cv::convertScaleAbs(distance_map, distance_map);    // conversion to 8 bit image
        while (rclcpp::ok() && !room_sequence_client.wait_server(5)) {
            std::cout << "room_sequence_client Action server not available after waiting, wait 5s..." << std::endl;
        }
        bool robot_start_coordinate_set = false;
        for (int v = 0; v < map_eroded.rows && robot_start_coordinate_set == false; ++v)
            for (int u = 0; u < map_eroded.cols && robot_start_coordinate_set == false; ++u)
                if (map_eroded.at<uchar>(v, u) != 0 && distance_map.at<uchar>(v, u) > 15) {
                    goal_seq.robot_start_coordinate.position.x =
                            u * goal_seq.map_resolution + goal_seg.map_origin.position.x;
                    goal_seq.robot_start_coordinate.position.y =
                            v * goal_seq.map_resolution + goal_seg.map_origin.position.y;
                    robot_start_coordinate_set = true;
                }

        std::shared_ptr<ClientSequence::Result> result_seq = room_sequence_client.sendGoal(goal_seq);
        if (result_seq == nullptr) {
            printf("Timeout on room segmentation.");
            return -1;
        }
        printf("Finished sequence planning successfully!");
    }

    //exit
    return 0;
}
